/*
 * "Copyright (c) 2007 The Regents of the University College Cork
 * All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose, without fee, and without written agreement is
 * hereby granted, provided that the above copyright notice, the following
 * two paragraphs and the author appear in all copies of this software.
 *
 * IN NO EVENT SHALL THE UNIVERSITY COLLEGE CORK BE LIABLE TO ANY PARTY FOR
 * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
 * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY 
 * COLLEGE CORK HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * THE UNIVERSITY COLLEGE CORK SPECIFICALLY DISCLAIMS ANY WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND THE UNIVERSITY COLLEGE CORK HAS NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
 */
 
/**
  * Generic Fifo Task queue. Reinstantiated for each task priority.
  * This component is controled by the PrioritySchedulerC.
  *
  * @param Number of tasks in the queue
  * @param Priority level of the task queue
  * @author Cormac Duffy 
  * @date   April 26 2007
  * @see    PrioritySchedulerC
  */ 
#include "TaskPriority.h"

generic module TaskQueueP(uint8_t NUM_TASKS, uint8_t PRIORITY) {
	provides interface TaskQueueControl;
	provides interface TaskBasic as PTask[uint8_t id];
	provides interface TaskDeadline<T32khz> as DTask[uint8_t id];
	uses interface LocalTime<T32khz>;
	uses interface Leds; 
}
implementation
{
  uint8_t head, tail;
  uint8_t next[NUM_TASKS];
  volatile uint32_t d_time[NUM_TASKS];
  

  inline bool isWaiting( uint8_t id){
    return (next[id] != NO_TASK) || (tail == id);
  }	
  
  inline bool pushTask(uint8_t id){
  	if( !isWaiting(id) ){
      		if(head == NO_TASK){
      			head = id;
			tail = id;
      		}
      		else{
			next[tail] = id;
			tail = id;
      		}
		return TRUE;
    	}
    	else
   		return FALSE;
  }

  inline uint8_t popTask(){
  	if( head != NO_TASK){
		uint8_t id = head;
		dbg("Scheduler", "POPTASK %hhu, priority %hhu ....\n",id);
     		head = next[head];
     	  
	      	if( head == NO_TASK ){
			tail = NO_TASK;
	      	}
	      	next[id] = NO_TASK;
	      	//process started is called here over runNextTask function, so that it stays in atomic
	      	if(PRIORITY == BASIC) signal TaskQueueControl.processStarted(0);
		else signal TaskQueueControl.processStarted(d_time[id]);
	      	return id;
	}    
	else
		return NO_TASK;
	
	return NO_TASK;
  }
  
  bool pushDTask( uint8_t id, uint32_t deadline ){
    if( !isWaiting(id) ){
      if( head == NO_TASK ){
		head = id;
		tail = id;
      }
      else{
		uint8_t t_curr = head;
		uint8_t t_prev = head;
		uint32_t local = call LocalTime.get();
		while (d_time[t_curr] - local <= deadline && t_curr != NO_TASK){
		  t_prev = t_curr;
		  t_curr = next[t_curr];
		}
		next[id] = t_curr;
		if (t_curr == head)
			head = id;
		else{
	  		next[t_prev] = id;
	  		if (t_curr == NO_TASK){ 
				tail = id;
	  		}
		}
      }
      d_time[id] = call LocalTime.get() + deadline;
      return TRUE;
    }
    else
      return FALSE;
  }

  async command uint32_t TaskQueueControl.getDeadline(){
	  atomic{return d_time[head];}
  }

  async command bool TaskQueueControl.emptyQueue(){
	  atomic{
		  if(head == NO_TASK) return TRUE;
		  else return FALSE;
	  }
  }
  
  command error_t TaskQueueControl.runNextTask(){
  	uint8_t nextTask;
    	atomic{nextTask=popTask();}
	if(nextTask==NO_TASK){
		return FALSE;
	}
	else{
		dbg("Scheduler", "Run Priority %hhu\n", PRIORITY);
	    	//move this statement into POP task so that it runs atomically!
	 	if( PRIORITY == BASIC ) signal PTask.runTask[nextTask]();
	 	else signal DTask.runTask[nextTask]();
	}
	return TRUE;
  }


 default event void PTask.runTask[uint8_t id](){}


  command void TaskQueueControl.initialize(){
  	int16_t i=0;
  	atomic{
  		if(NUM_TASKS){
     		tail = NO_TASK;
	  	head = NO_TASK;
      	  	for(i=0;i<NUM_TASKS;i++)
	      		next[i]=NO_TASK;
       	  	}
  	}
  }


  async command error_t PTask.postTask[uint8_t id](){
     error_t result;
     atomic {
     	result =  pushTask(id) ? SUCCESS : EBUSY;
     	if(result==SUCCESS)	
     		signal TaskQueueControl.queueNotify();
    	}
    	return result;
  }
  
  async command error_t DTask.postTask[uint8_t id](uint32_t deadline){
    error_t result;
    atomic{
	result = pushDTask(id, deadline) ? SUCCESS : EBUSY;
	if(result == SUCCESS)
		signal TaskQueueControl.queueNotify();
	}
	return result;
  }
  
	default event void DTask.runTask[uint8_t id]() {}
}
